Example of a WOLF ROS2 Application
The aim of this section is to describe how to configure a WOLF application in ROS2, providing an example of using WOLF to solve a real SLAM problem. This example considers a differential drive robot equipped with a 2D laser whose measurements will be fused with the odometry (see Demo Laser 2d). The sensor data will be streamed to WOLF using ROS2.
Along with the WOLF plugins and packages, we provide a generic ROS2 node that will serve for most robotics projects without modification. This means that all the user has to do is:
Write some YAML files specifying the problem.
Write a launch file for your application specifying the path to your root YAML file, rviz, bags, etc.
Launch and enjoy.
The node will load all necessary WOLF plugins and WOLF ROS packages, and initialize the WOLF tree.
Note
Any other demo that you want to run will differ from this one
ONLY in these .yaml and .launch configuration files.
Note
There is no need to modify any CMakeLists.txt or package.xml files, or to write any C++ code. The WOLF ROS2 node will automatically load the necessary WOLF plugins and packages in runtime based on the provided YAML configuration files.
Find more extensive documentation on how to configure each component in the YAML configuration section.
The YAML configuration file(s)
In the example of the demo laser 2D, the YAML file provided specifies a robot with two sensors:
a 2D odometer
a 2D laser range scanner.
It has however three processors:
a 2D odometry integrator, attached to the odometry sensor
A laser odometry processor (using ICP), attached to the laser sensor
A laser loop closure processor (using ICP), attached to the laser sensor
The system uses one subscriber for odometry and another one for laser scans.
It requires the WOLF plugin laser, and the ROS
subscribers SubscriberOdom2d and SubscriberLaser2d available in the packages
WOLF ROS2 node and WOLF ROS2 laser.
Here’s the YAML configuration file of the demo:
node:
rate: 100.0
profiling:
enabled: true
file: "~/wolf_ros2_demo_laser2d_profiling.txt"
print:
enabled: true
period: 5 # only if enabled
depth: 4 # only if enabled
state: true # only if enabled
factored_by: true # only if enabled
metric: true # only if enabled
state_blocks: true # only if enabled
problem:
dimension: 2
tree_manager:
type: "none"
first_frame:
P:
value: [0.0, 0.0]
prior:
mode: "fix"
O:
value: [0.0]
prior:
mode: "fix"
map:
type: "MapBase"
plugin: "core"
solver:
follow: "solver.yaml"
sensors:
-
type: "SensorOdom2d"
name: "sensor_odom"
plugin: "core"
states:
P:
dynamic: false
value: [0,0]
prior:
mode: fix
O:
dynamic: false
value: [0]
prior:
mode: fix
min_disp_var: 1e-6
min_rot_var: 1e-6
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
k_rot_to_rot: 0.1
-
type: "SensorLaser2d"
name: "laser_scan"
plugin: "laser"
states:
P:
dynamic: false
value: [0,0]
prior:
mode: fix
O:
dynamic: false
value: [0]
prior:
mode: fix
angle_min: -2.35619 # only used if: ROS subscriber/SubscriberLaser2d/load_params_from_msg = false
angle_max: 2.35619 # only used if: ROS subscriber/SubscriberLaser2d/load_params_from_msg = false
angle_step: 0.00436332 # only used if: ROS subscriber/SubscriberLaser2d/load_params_from_msg = false
scan_time: 0.025 # only used if: ROS subscriber/SubscriberLaser2d/load_params_from_msg = false
range_min: 0.023 # only used if: ROS subscriber/SubscriberLaser2d/load_params_from_msg = false
range_max: 60 # only used if: ROS subscriber/SubscriberLaser2d/load_params_from_msg = false
range_std_dev: 0.2
angle_std_dev: 0.01
processors:
-
type: "ProcessorOdom2d"
name: "processorodom"
sensor_name: "sensor_odom"
plugin: "core"
time_tolerance: 0.01
apply_loss_function: false
debug_verbose_level: "none"
keyframe_vote:
voting_active: false
max_time_span: 10
max_buff_length: 0
max_dist_traveled: 1
max_angle_turned: 0.5
max_cov_det: 1e3
unmeasured_perturbation_std: 1e-3
state_provider: true
state_provider_order: 1
-
type: "ProcessorOdomIcp"
name: "processorodomicp"
sensor_name: "laser_scan"
plugin: "laser"
time_tolerance: 0.01
dimension: 2
debug_verbose_level: "none"
keyframe_vote:
voting_active: true
dist_traveled: 1
angle_turned: 0.5
time_span: 5.0
invalid_icp: false
consecutive_invalid_icp_patience : 1.0
consecutive_invalid_icp_timeout : 5.0
icp:
follow: "csm.yaml"
initial_guess: "odom"
apply_loss_function: false
state_provider: true
state_provider_order: 2
-
type: "ProcessorLoopClosureIcp"
name: "processorloopclosureicp"
sensor_name: "laser_scan"
plugin: "laser"
time_tolerance: 0.01
apply_loss_function: true
debug_verbose_level: "none"
keyframe_vote:
voting_active: false
recent_frames_ignored: 3
frames_ignored_after_loop: 0
max_error_threshold: 0.02
min_points_percent: 40
max_loops: 3
max_candidates: 5
max_attempts: 15
candidate_generation: "random" # 'random' or 'tree'
icp:
follow: "csm.yaml"
subscribers:
-
package: "wolf_ros2_laser"
type: "SubscriberLaser2d"
topic: "/base_scan"
sensor_name: "laser_scan"
load_params_from_msg: true
-
package: "wolf_ros2_node"
type: "SubscriberOdom2d"
topic: "/odom"
sensor_name: "sensor_odom"
odom_from_pose: false
publishers:
-
package: "wolf_ros2_node"
type: "PublisherTf"
topic: " "
period: 0.1
map_frame_id: "map"
odom_frame_id: "odom"
base_frame_id: "base_footprint"
broadcast_odom: false
broadcast_extrinsics: false
-
package: "wolf_ros2_node"
type: "PublisherGraph"
topic: "graph"
period: 1
-
package: "wolf_ros2_laser"
type: "PublisherLaserMap"
topic: "map"
period: 1
map_frame_id: "map"
update_dist_th: 0.05
update_angle_th: 0.05
max_n_cells: 1000000
grid_size: 0.05
p_free: 0.3
p_obst: 0.8
p_free_th: 0.2
p_obst_th: 0.9
discard_max_range: true
The two ICP processors use internally the Canonical Scan Matcher or CSM.
In this case, the file contains almost all parameters in a plain YAML file.
However, the follow key can be used to include the content
of another YAML file into the current one (it is copy-pasted in runtime).
It is the case for the parameters under icp of the ProcessorOdomIcp and ProcessorLoopClosureIcp,
and under solver in the example above.
If you are
familiar with CSM, you may be able to identify some of the CSM
parameters in the YAML configuration file csm.yaml:
verbose: false # prints debug messages
# ALGORITHM OPTIONS
use_point_to_line_distance: true # use PlICP (true) or use vanilla ICP (false).
max_angular_correction_deg: 180 # Maximum angular displacement between scans (deg)
max_linear_correction: 10 # Maximum translation between scans (m)
max_correspondence_dist: 0.5 # Maximum distance for a correspondence to be valid
use_corr_tricks: true # Use smart tricks for finding correspondences. Only influences speed; not convergence.
debug_verify_tricks: false # Checks that find_correspondences_tricks give the right answer
# STOP CRITERIA
max_iterations: 50 # maximum iterations
epsilon_xy: 1e-4 # distance change
epsilon_theta: 1e-3 # angle change
# RESTART
restart: false # Restart algorithm
restart_threshold_mean_error: 0 # Threshold for restarting
restart_dt: 0 # Displacement for restarting
restart_dtheta: 0 # Displacement for restarting
# DISCARDING POINTS/CORRESPONDENCES
min_reading: 0.023 # discard rays outside of this interval
max_reading: 60 # discard rays outside of this interval
outliers_maxPerc: 0.9 # Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error
outliers_adaptive_order: 0.7 # Parameters describing a simple adaptive algorithm for discarding
outliers_adaptive_mult: 1.5 # Parameters describing a simple adaptive algorithm for discarding
outliers_remove_doubles: false # Do not allow two different correspondences to share a point
do_visibility_test: false # If initial guess, visibility test can discard points that are not visible
do_alpha_test: false # Discard correspondences based on the angles
do_alpha_test_thresholdDeg: 10 #
# POINT ORIENTATION
clustering_threshold: 0.5 # Max-distance clustering for point orientation
orientation_neighbourhood: 4 # Number of neighbour rays used to estimate the orientation
# WEIGHTS
use_ml_weights: false # weight the impact of each correspondence. This works fabolously if the first scan has no noise.
use_sigma_weights: false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2
sigma: 0.2 # Noise of the scan
# COVARIANCE
do_compute_covariance: true
cov_factor: 5 # Factor multiplying the cov output of csm
cov_max_eigv_factor: 5 # Factor multiplying the direction of the max eigenvalue of the cov output of csm
# VALIDATION & ATTEMPTS
attempts: 3 # number of icp attempts if result fails (not valid or error > restart_threshold_mean_error) - TYPE unsigned int
perturbation_new_attempts: 1e-4 # MANDATORY if $attempts > 1: perturbation noise amplitude applied to initial guess from 2nd icp attempt - TYPE double
max_mean_error: 0.01 # Max mean error to consider the ICP result valid - TYPE double
min_points_ratio: 0.5 # Min ratio of points with correspondence to consider the ICP result valid - TYPE double
To know more about the YAML configuration files, please refer to the YAML schema section.
Launch file
To facilitate running the WOLF ROS2 node, visualization (RVIZ), reproducing data (bag), it is useful to create a launch file. Here you can observe the launch file for the laser 2D demo example.
#!/usr/bin/env python3
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.actions import RegisterEventHandler, EmitEvent
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from launch.conditions import IfCondition
from launch.substitutions import (
LaunchConfiguration,
PathJoinSubstitution,
TextSubstitution,
)
from launch_ros.actions import Node
def generate_launch_description():
# Launch arguments
rviz_arg = DeclareLaunchArgument(
"rviz", default_value="true", description="Enable RViz visualization"
)
speed_arg = DeclareLaunchArgument(
"speed", default_value="1", description="Playback speed for rosbag"
)
yaml_arg = DeclareLaunchArgument(
"yaml",
default_value="demo_laser2d.yaml",
description="YAML configuration file name (with the extension)",
)
bag_arg = DeclareLaunchArgument(
"bag",
default_value="advanced",
description="Bag file name (basic, advanced, turtlebot)",
)
# Get package share directory
wolf_demo_pkg_share = get_package_share_directory("wolf_ros2_demo_laser2d")
# WOLF node - CORRECTED PARAMETER
wolf_node = Node(
package="wolf_ros2_node",
executable="wolf_ros2_node",
name="wolf_ros2_node",
output="screen",
parameters=[
{
"yaml_file_path": PathJoinSubstitution(
[wolf_demo_pkg_share, "yaml", LaunchConfiguration("yaml")]
),
"use_sim_time": True,
}
],
)
# RViz visualization
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz",
arguments=[
"-d",
PathJoinSubstitution([wolf_demo_pkg_share, "rviz", "demo_laser2d.rviz"]),
],
condition=IfCondition(LaunchConfiguration("rviz")),
)
# Rosbag play
bag_play = ExecuteProcess(
cmd=[
"ros2",
"bag",
"play",
"--clock",
"-r",
LaunchConfiguration("speed"),
PathJoinSubstitution(
[wolf_demo_pkg_share, "bag", LaunchConfiguration("bag")]
),
],
output="screen",
)
# Event handler to shutdown when rosbag play completes
shutdown_on_bag_complete = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=bag_play, on_exit=[EmitEvent(event=Shutdown())]
)
)
return LaunchDescription(
[
rviz_arg,
speed_arg,
yaml_arg,
bag_arg,
wolf_node,
rviz_node,
bag_play,
shutdown_on_bag_complete,
]
)